#! /usr/bin/env python
#-*- coding: utf-8 -*-

import matplotlib
matplotlib.use('GTKCairo')
import matplotlib.pyplot as plt
import numpy as np
import pylab
import math

from scipy import *
from pylab import *

draft = False

x_min = -0.8
x_max =  0.675
y_min = -0.55
y_max =  0.475

aspect_ratio = (x_max-x_min)/(y_max-y_min)
h_size = 16.00

my_title_spec = {'color'    : 'k', 'fontsize'   : 18 }
fig = plt.figure(figsize=(h_size, h_size/aspect_ratio))
fig.subplots_adjust(left=0.05, right=0.95, bottom=0.05, top=0.95,
                    hspace=0.25, wspace=0.2)
eq_fs = 20

ax = pylab.gca()


# frame
arrow(-0.7,-0.45,1.0,0, width=0.0025)
arrow(-0.7,-0.45,0,0.5, width=0.0025)
lz = Line2D([-0.7,0.0],[0.,0.], color='k', linewidth=0.5, linestyle='--')
ax.add_line(lz)
lx = Line2D([0.0,0.0],[0.,-0.45], color='k', linewidth=0.5, linestyle='--')
ax.add_line(lx)
plt.text(-0.75, -0.01, '$z$' , fontsize=40)
plt.text(-0.015,-0.5, '$x$' , fontsize=40)


# mass
arrow( 0.0,0,0,-0.35,  width=0.005)
plt.text(0.05, -0.3, '$mg$' , fontsize=30)

# body
lb = Line2D([-0.5,0.5],[0.,0.], color='k', linewidth=0.5, linestyle='--')
body = [lb]
rb = matplotlib.patches.Rectangle((-0.5,-0.04), 1, 0.01, color="grey", alpha=0.50)
body.append(rb)
# shaft 1
sh1 = matplotlib.patches.Rectangle((-0.5,-0.03), 0.01, 0.02, color="grey", alpha=0.50)
body.append(sh1)
# shaft 2
sh2 = matplotlib.patches.Rectangle((0.5,-0.03), -0.01, 0.02, color="grey", alpha=0.50)
body.append(sh2)
# center sphere
bd1 = matplotlib.patches.Ellipse((0., 0.02), 0.15, 0.15, color="grey", alpha=0.50)
body.append(bd1)
# landing gear
lg1 = matplotlib.patches.Rectangle((0.045,-0.03), 0.01, -0.1, color="grey", alpha=0.50)
body.append(lg1)
lg2 = matplotlib.patches.Rectangle((-0.045,-0.03), -0.01, -0.1, color="grey", alpha=0.50)
body.append(lg2)

t30 = mpl.transforms.Affine2D().rotate_deg(30) + ax.transData
for p in body:
    p.set_transform(t30)
    ax.add_line(p)

# rotors
rr1 = matplotlib.patches.Rectangle((-0.7,-0.01), 0.4, 0.02, color="green", alpha=0.50)
rr1.set_transform(t30)
ax.add_line(rr1)
arrow( -0.5,0,0,0.2,  width=0.005, transform=t30)
plt.text(-0.45, -0.1, '$F_1$' , fontsize=30)

rr2 = matplotlib.patches.Rectangle((0.7,-0.01), -0.4, 0.02, color="green", alpha=0.50)
rr2.set_transform(t30)
ax.add_line(rr2)
arrow( 0.5,0,0,0.2,  width=0.005, transform=t30)
plt.text(0.4, 0.4, '$F_2$' , fontsize=30)


# theta
plt.text(0., 0.025, '$l$' , fontsize=30)
plt.text( 0.28, 0.05 , '$\\theta$' , fontsize=40)
lt = Line2D([0.,0.4],[0.,0.], color='k', linewidth=0.5, linestyle='--')
ax.add_line(lt)
arrow( 0.25,0.02,0,0.1,  width=0.0025)

plt.ylim(y_min, y_max)
plt.xlim(x_min, x_max)

if not draft:
    ax.axes.get_xaxis().set_visible(False)
    ax.axes.get_yaxis().set_visible(False)

#filename = "../images/planar_vtol/schematics.png"
#plt.savefig(filename, dpi=120)

plt.show()
